#ifndef ___MOTOR_H___
#define ___MOTOR_H___
#include <time.h>


#include "NXT++.h"
#include <iostream>
#include <ctime>
#include <cmath>

#define MOTOR_W 141 // 1/2 the width of the wheelbase, in mm
#define MOTOR_L 127 // 1/2 the length of the wheelbase, in mm
#define TIC_PER_MM 11.5//8.7 // mm of travel/encoder tick
#define TIC_PER_SEC_PER_PWR 40 // ticks/second/pwr increment
// 1000 degrees/second at pwr=100 / 100 * 1 revolution/ 360 degrees * 1440 ticks/revolution
#define TOP_SPEED 100*TIC_PER_SEC_PER_PWR/TIC_PER_MM // highest representable speed in mm/s
#define PI 3.1459
/* Here are the defines for the motor speed setting retries. */
// Try to get it moving 5 times before giving up..
#define MOTOR_COMMAND_RETRIES 5
// How far in millimeters should we wait for a motor to move before
// checking up on it
#define MOTOR_CHECK_DISTANCE_MM 5
#define MOTOR_ENCODER_CHECK_THRESHOLD (TIC_PER_MM * MOTOR_CHECK_DISTANCE_MM / 2)
// motor 2 is on the left
// speeds are in mm/s, distances are in mm
// angles are in degrees

struct position 
{
	double left;
	double right;
};

class ShrMotor
{
	
	public:
		//functions
		//ShrMotor();
		//~ShrMotor();
			// initalise the motor
		void setup(int port=IN_1);
		// reset the encoders
		void resetPosition();
		void sleep(unsigned int mseconds);
		// return the distance travelled by each wheel
		position getPosition();
		// sets the motors to constant speed mode
		void setModeSpeed();
		// stops the motors.
		void stop();
		// sets the motors to servo mode
		void setModeServo();
		// sets the speed of both motors. takes care of unit conversion, and overspeed correction.
		// TODO: slippage correction to emulate 2wheeled differential drive
		void setSpeed(double spdLeft, double spdRight);
		// sets the target position of bot motors in ticks and confirms the target was	actually stored. target is relative to current position.
		void setTargetTicks(int trgLeft, int trgRight);
		// sets the target position of both motors. takes care of unit conversion. Target is relative to current position.
		// TODO: slippage correction to emulate 2wheeled differential drive
		void setTarget(double trgLeft, double trgRight);
		// return the current encoder target
		// for debugging
		position getTarget();
		// returns the battery voltage in volts
		double getVoltage();
		// drive forward at the specified speed
		void drive(double speed = 100);
		// drive forward at the specified speed for the specified distance
		void driveTo(double dist, double speed = 100);
		// turn counterclockwise in a circle of the specified radius driving at the specified speed.
		// TODO: test, explain how the math works
		void curve(double radius, double speed = 100);
		// turn in the specified direction at the specified speed
		void turn(double direction, double revolutionsPerSecond = .1);
		// turn counterclockwise the specified angle at the specified speed.
		// use busy to check for done
		void turnAngle(double angle, double revolutionsPerSecond = 0.1);
		// returns true if the motors are en route to the target postion
		// for use in servo mode
		int busy();
		// internal use
		void waitMS(int ms);
		void i2cWrite(int address, int data);
		int i2cRead(int address, int bytes = 1);
		int speed2pwr(double speed);
		int dist2tic(double mm);

	//comm object
		Comm::NXTComm comm; //create NXTcomm object to use
	//data

		float motorWidth;
		float motorLength;
		float encoderTickMM;
		float tickPSecond;
		int motor_tries;
		static const int SETINPUTMODE  = 0x05;
		static const int LOWSPEED9V = 0x0B;
		static const int RAWMODE = 0x00;
		static const int LSWRITE = 0x0F;
		static const int LSREAD = 0x10;
		static const int BATT_V = 0x54;
		static const int M1_MODE = 0x44;
		static const int M1_PWR = 0x45;
		static const int M1_ENC = 0x4C;
		static const int M1_TRG = 0x40;
		static const int M2_MODE = 0x47;
		static const int M2_PWR = 0x46;
		static const int M2_ENC = 0x50;
		static const int M2_TRG = 0x48;
		static const int MODE_MSK = 0xFC; // mask for the mode select bits
		static const int MODE_PWM = 0x0;
		static const int MODE_SPD = 0x1;
		static const int MODE_SRV = 0x2;
		static const int MODE_REV = 0x8;
		static const int MODE_RST = 0x3;
		static const int MODE_BSY = 0x80;
		static const int MODE_NTO = 0x0; // no disable timeout
		//0b00010000 // disable timeout
};

#endif // ___SCHRODIE_MOTOR_H___